//
// Created by engineai on 2024/07/03.
//

#ifndef ZQ_HUMANOID_UTILITIES_H
#define ZQ_HUMANOID_UTILITIES_H

#include <algorithm>
#include <map>
#include <random>
#include <unordered_map>
#include <vector>
#include "../cppTypes.h"

/*!
 * Are two floating point values almost equal?
 * @param a : first value
 * @param b : second value
 * @param tol : equality tolerance
 */
template <typename T>
bool fpEqual(T a, T b, T tol)
{
    return std::abs(a - b) <= tol;
}

/*!
 * Are two std::vectors equal?
 * Compares with "!=" operator
 */
template <typename T>
bool vectorEqual(const std::vector<T> &a, const std::vector<T> &b)
{
    if (a.size() != b.size())
    {
        return false;
    }
    for (size_t i = 0; i < a.size(); i++)
    {
        if (a[i] != b[i])
        {
            return false;
        }
    }
    return true;
}

/*!
 * Coerce in to be between min and max
 */
template <typename T>
T coerce(T in, T min, T max)
{
    if (in < min)
    {
        in = min;
    }
    if (in > max)
    {
        in = max;
    }
    return in;
}

/*!
 * Apply deadband
 * @param x : input
 * @param range : deadband (+/- range around 0)
 * @return result
 */
template <typename T>
T deadband(T x, T range)
{
    if (x < range && x > -range)
    {
        x = T(0);
    }
    return x;
}

/*!
 * Apply deadband to eigen type
 */
template <typename T>
void eigenDeadband(Eigen::MatrixBase<T> &v, typename T::Scalar band)
{
    for (size_t i = 0; i < T::RowsAtCompileTime; i++)
    {
        for (size_t j = 0; j < T::ColsAtCompileTime; j++)
        {
            v(i, j) = deadband(v(i, j), band);
        }
    }
}

/*!
 * Get the sign of a number
 * 1 for positive, 0 for 0, -1 for negative...
 */
template <typename T>
int sgn(T val)
{
    return (T(0) < val) - (val < T(0));
}

/*!
 * Fill an eigen type with random numbers from a random generator and uniform
 * real distribution.
 * TODO: is there a way to make this work nicely with normal distributions too?
 */
template <typename T>
void fillEigenWithRandom(
    Eigen::MatrixBase<T> &v, std::mt19937 &gen,
    std::uniform_real_distribution<typename T::Scalar> &dist)
{
    for (size_t i = 0; i < T::RowsAtCompileTime; i++)
    {
        for (size_t j = 0; j < T::ColsAtCompileTime; j++)
        {
            v(i, j) = dist(gen);
        }
    }
}

/*!
 * Generate a random number following normal distribution
 */
template <typename T>
T generator_gaussian_noise(T mean, T var)
{
    static bool hasSpare = false;
    static T rand1, rand2;

    if (hasSpare)
    {
        hasSpare = false;
        return mean + sqrt(var * rand1) * sin(rand2);
    }
    hasSpare = true;

    rand1 = rand() / ((T)RAND_MAX);
    if (rand1 < 1e-100)
    {
        rand1 = 1e-100;
    }
    rand1 = -2 * log(rand1);
    rand2 = rand() / ((T)RAND_MAX) * M_PI * 2.;

    // printf("rand: %f, %f\n", rand1, rand2);
    return mean + sqrt(var * rand1) * cos(rand2);
}

/*!
 * Does the unordered map contain the given element?
 */
template <typename T1, typename T2>
bool uMapContains(const std::unordered_map<T1, T2> &set, T1 key)
{
    return set.find(key) != set.end();
}

/*!
 * Does the unordered map contain the given element?
 */
template <typename T1, typename T2>
bool mapContains(const std::map<T1, T2> &set, T1 key)
{
    return set.find(key) != set.end();
}

/*!
 * Convert a floating point number to a string.  Is preferable over
 * std::to_string because this uses scientific notation and won't truncate
 * small/large numbers.
 */
template <typename T>
std::string numberToString(T number)
{
    static_assert(std::is_floating_point<T>::value,
                  "numberToString must use a floating point type!");
    char buffer[100];
    sprintf(buffer, "%g", number);
    return std::string(buffer);
}

/*!
 * map value x in (inputMin, inputMax) to (outputMin, outputMax) linearly
 */
template <typename T>
T mapToRange(T x, T inputMin, T inputMax, T outputMin, T outputMax)
{
    return outputMin +
           (x - inputMin) * (outputMax - outputMin) / (inputMax - inputMin);
}

/*!
 * Convert eigen type to std::string.
 */
template <typename T>
std::string eigenToString(Eigen::MatrixBase<T> &value)
{
    std::stringstream ss;
    ss << value;
    return ss.str();
}

/*!
 * Convert boolean to string (true, false)
 */
static inline std::string boolToString(bool b)
{
    return std::string(b ? "true" : "false");
}

void writeStringToFile(const std::string &fileName,
                       const std::string &fileData);
std::string getCurrentTimeAndDate();
std::string getConfigDirectoryPath();

/*!
 * Get the rotation matrix coincide with euler angle
 * Intrisic ZYX rotation
 */
template <typename T>
void EulerZYX_2_SO3(const Vec3<T> &euler_zyx, Mat3<T> &SO3)
{
    Mat3<T> Mat3_Z, Mat3_Y, Mat3_X;
    Mat3_Z << cos(euler_zyx[0]), -sin(euler_zyx[0]), 0, sin(euler_zyx[0]),
        cos(euler_zyx[0]), 0, 0, 0, 1;
    Mat3_Y << cos(euler_zyx[1]), 0, sin(euler_zyx[1]), 0, 1, 0,
        -sin(euler_zyx[1]), 0, cos(euler_zyx[1]);
    Mat3_X << 1, 0, 0, 0, cos(euler_zyx[2]), -sin(euler_zyx[2]), 0,
        sin(euler_zyx[2]), cos(euler_zyx[2]);

    SO3 = Mat3_Z * Mat3_Y * Mat3_X;
}

// Smooth Changing
/*!
 * Interpolate with cosine (sometimes called coserp)
 */
template <typename T>
T smooth_change(T ini, T end, T moving_duration, T curr_time)
{
    if (curr_time > moving_duration)
    {
        return end;
    }
    return (ini +
            (end - ini) * 0.5 * (1 - cos(curr_time / moving_duration * M_PI)));
}

/*!
 * Derivative of smooth_change
 */
template <typename T>
T smooth_change_vel(T ini, T end, T moving_duration, T curr_time)
{
    if (curr_time > moving_duration)
    {
        return 0.0;
    }
    return ((end - ini) * 0.5 * (M_PI / moving_duration) *
            sin(curr_time / moving_duration * M_PI));
}

/*!
 * Derivative of smooth_change_vel
 */
template <typename T>
T smooth_change_acc(T ini, T end, T moving_duration, T curr_time)
{
    if (curr_time > moving_duration)
    {
        return 0.0;
    }
    return ((end - ini) * 0.5 * (M_PI / moving_duration) *
            (M_PI / moving_duration) * cos(curr_time / moving_duration * M_PI));
}

/*!
 * Convert from string to float or double.
 */
template <typename T>
T stringToNumber(const std::string &str)
{
    static_assert(std::is_same<T, double>::value || std::is_same<T, float>::value,
                  "stringToNumber only works for double/float");

    if (std::is_same<T, double>::value)
    {
        return std::stod(str);
    }
    else if (std::is_same<T, float>::value)
    {
        return std::stof(str);
    }
}

/*!
 * Convert from string to float or double
 */
template <typename T>
T stringToNumber(const char *str)
{
    return stringToNumber<T>(std::string(str));
}

/*!
 * Convert from string to Vec3.
 */
template <typename T>
Vec3<T> stringToVec3(const std::string &str)
{
    Vec3<T> v;
    size_t i = 0;

    // seek past whitespace
    while (str.at(i) == ' ')
    {
        i++;
    }

    if (str.at(i) == '[')
    {
        i++;
    }
    else
    {
        throw std::runtime_error("stringToVec3 didn't find open bracket");
    }

    // seek past whitespace
    while (str.at(i) == ' ')
    {
        i++;
    }
    size_t start = i;

    // seek to end of first number
    while (str.at(i) != ',')
    {
        i++;
    }
    v[0] = stringToNumber<T>(str.substr(start, i - start));
    i++;

    while (str.at(i) == ' ')
    {
        i++;
    }
    start = i;
    while (str.at(i) != ',')
    {
        i++;
    }
    v[1] = stringToNumber<T>(str.substr(start, i - start));
    i++;

    while (str.at(i) == ' ')
    {
        i++;
    }
    start = i;
    while (str.at(i) != ']')
    {
        i++;
    }
    v[2] = stringToNumber<T>(str.substr(start, i - start));
    return v;
}

std::string getLcmUrl(s64 ttl);

#endif // ZQ_HUMANOID_UTILITIES_H
